﻿
// 2023/11/15: 基于v6 2023/10/30版本。新增z轴相对速度、绝对速度、相对加速度、绝对加速度字段
// 2025/8/7: 新增ObjectClass.WheelBarrow, TruckHead, TrafficSign, TrafficLight

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;
using SO = System.Reflection.ObfuscationAttribute;

namespace ObjSensorSampleV7
{
    [SO]
    enum ObjectClass
    {
        General = 1, // 一般物体大类
        Car = 2, // 车辆大类
        Pedestrian = 3, // 行人大类
        Truck = 4, // 货车大类
        Bike = 5, // Bike大类
        RailCar = 6, // 轨道车辆大类
        Special = 7, // 特殊物体大类
        RoadStatic = 8, // 道路内静态物体大类
        SideStatic = 9, // 道路外静态物体大类

        GeneralSmall = 11, // 一般小物体
        GeneralBig = 12, // 一般大物体
        Barrier = 13, // 一般障碍物

        Van = 21, // 面包车
        Minibus = 22, // 小巴
        Bus = 23, // 大巴
        BatteryCart = 24, // 园区电瓶车
        TinyCar = 25, // 微型车
        SUV = 26, // SUV

        Adult = 31, // 成人
        Child = 32, // 小孩
        Scooter = 33, // 平衡车
        WheelChair = 34, // 轮椅
        WheelBarrow = 35, // 手推车

        Minitruck = 41, // 小卡车
        ContainerTruck = 42, // 货柜车（货柜车头 + 拖车）
        SpecialCar = 43, // 特种车辆
        Trailer = 44, // 拖车
        TruckHead = 45, // 货柜车头

        Motorbike = 51, // 摩托车
        Bicycle = 52, // 自行车
        ElectricBike = 53, // 电瓶自行车
        Tricycle = 54, // 三轮车

        Train = 61, // 火车
        Tram = 62, // 有轨电车

        Animal = 71, // 动物
        Ball = 72, // 球类
        Litter = 73, // 垃圾等杂物

        Cone = 81, // 锥形路障
        ManholeCover = 82, // 井盖
        Patch = 83, // 路面补丁
        Gantry = 84, // 龙门架

        Pole = 91, // 竖杆
        Tree = 92, // 树木
        Vegetation = 93, // 灌木
        Building = 94, // 建筑物
        TrafficSign = 95, // 交通标志
        TrafficLight = 96, // 交通灯
    }

    [SO]
    enum PositionMode
    {
        ClosestPoint = 1, // 最近点
        BoxCenter = 2, // 框中心
    }

    class ObjectInfo
    {
        public int ID { get; set; } // ID of the object / 目标物ID
        public int Age { get; set; } // Age of the object / 目标物的Age
        public int? RawID { get; set; } // Raw ID of the object / 目标物的原始ID
        public int? RawAge { get; set; } // Raw age of the object / 目标物的原始Age
        public double? Confidence { get; set; } // [%] Confidence (existence) of the object / 目标物的置信度

        public int? TimeOffset { get; set; } // [us] Time offset (to the frame's time) / 时间偏置(相对于帧的时间戳)
        public ColorRGBA? Color { get; set; } // Color of the object / 目标物的颜色

        public ObjectClass ObjectClass { get; set; } // Classification of the object / 目标物的类别
        public int? ObjectRawClassID { get; set; } // Raw classification ID of the object / 目标物的原始类别ID
        public double? ObjectClassConfidence { get; set; } // [%] Confidence (classification) of the object / 目标物的分类置信度

        public PositionMode PosMode { get; set; } // Position mode of the object 目标物的位置模式
        public double PosX { get; set; } // [m] X-axis coordination of the object's position / 目标物的x轴方向位置
        public double PosY { get; set; } // [m] Y-axis coordination of the object's position / 目标物的y轴方向位置
        public double? PosZ { get; set; } // [m] Z-axis coordination of the object's position / 目标物的z轴方向位置
        public double? PosXSigma { get; set; } // [m] X-axis sigma of the object's position / 目标物x轴方向位置的精度
        public double? PosYSigma { get; set; } // [m] Y-axis sigma of the object's position / 目标物y轴方向位置的精度
        public double? PosZSigma { get; set; } // [m] Z-axis sigma of the object's position / 目标物z轴方向位置的精度

        public double CPX { get; set; } // [m] X-axis coordination of the object's closest point / 目标物的最近点x轴坐标
        public double CPY { get; set; } // [m] Y-axis coordination of the object's closest point / 目标物的最近点y轴坐标
        public double CPD { get; set; } // [m] Distance between the object's closest point and subject vehicle's contour / 目标物的最近点与本车轮廓距离

        public double? VXAbs { get; set; } // [KPH] X-axis absolute velocity of the object / x轴方向绝对速度
        public double? VYAbs { get; set; } // [KPH] Y-axis absolute velocity of the object / y轴方向绝对速度
        public double? VZAbs { get; set; } // [KPH] Z-axis absolute velocity of the object / z轴方向绝对速度
        public double? AXAbs { get; set; } // [m/s²] X-axis absolute acceleration of the object / x轴方向绝对加速度
        public double? AYAbs { get; set; } // [m/s²] Y-axis absolute acceleration of the object / y轴方向绝对加速度
        public double? AZAbs { get; set; } // [m/s²] Z-axis absolute acceleration of the object / z轴方向绝对加速度
        public double? VXRel { get; set; } // [KPH] X-axis relative velocity of the object / x轴方向相对速度
        public double? VYRel { get; set; } // [KPH] Y-axis relative velocity of the object / y轴方向相对速度
        public double? VZRel { get; set; } // [KPH] Z-axis relative velocity of the object / z轴方向相对速度
        public double? AXRel { get; set; } // [m/s²] X-axis relative acceleration of the object / x轴方向相对加速度
        public double? AYRel { get; set; } // [m/s²] Y-axis relative acceleration of the object / y轴方向相对加速度
        public double? AZRel { get; set; } // [m/s²] Z-axis relative acceleration of the object / z轴方向相对加速度

        public double? YawRate { get; set; } // [deg/s] Yaw rate of the object / 横摆角速度
        public double? Curvature { get; set; } // [1/m] Turning curvature of the object / 转弯曲率

        public double? Heading { get; set; } // [deg] Orientation of the object / 目标物的朝向
        public double? Width { get; set; } // [m] Width of the object's box / 目标物的宽度
        public double? Length { get; set; } // [m] Length of the object's box / 目标物的长度
        public double? Height { get; set; } // [m] Height of the object's box / 目标物的高度
        public FloatPoint[] Trajectory { get; set; } // List of object's trajectory points / 目标物轨迹点的列表
        public FloatPoint[] Contour { get; set; } // List of object's contour points / 目标物轮廓点的列表

        public double? RCS { get; set; } // [dB] Radar-cross-section of the radar target / 雷达目标物的雷达散射截面
        public double? VLng { get; set; } // [m/s] Longitudinal velocity of the radar target / 雷达目标物的径向速度
        public double? VLat { get; set; } // [m/s] Lateral velocity of the radar target / 雷达目标物的切向速度

        public FloatPoint ObjectPosition // Get object's potision point / 获取目标物位置点
        {
            get { return new FloatPoint((float)PosX, (float)PosY); }
        }

        public FloatPoint ClosestPoint // Get object's closest point / 获取目标物最近点
        {
            get { return new FloatPoint((float)CPX, (float)CPY); }
        }

        public double? Speed // Get absolute speed / 获取绝对车速 [KPH]
        {
            get
            {
                if (VXAbs != null && VYAbs != null)
                {
                    if (VZAbs != null) return Math.Sqrt(VXAbs.Value * VXAbs.Value + VYAbs.Value * VYAbs.Value + VZAbs.Value * VZAbs.Value);
                    else return Math.Sqrt(VXAbs.Value * VXAbs.Value + VYAbs.Value * VYAbs.Value);
                }
                else return null;
            }
        }

        public ObjectClass ObjectCategory // Get object's category / 获取目标物大类
        {
            get
            {
                if ((int)ObjectClass < 10) return ObjectClass;
                else return (ObjectClass)((int)ObjectClass / 10);
            }
        }

        public ObjectInfo()
        {
            PosMode = PositionMode.ClosestPoint;
            ObjectClass = ObjectClass.General;
        }
    }

    class FovDescription
    {
        public double PositionX { get; set; } // [m] X-axis coordination of the FOV's center point / FOV中心点x轴坐标
        public double PositionY { get; set; } // [m] Y-axis coordination of the FOV's center point / FOV中心的y轴坐标
        public double AngleRange { get; set; } // [deg] Angle range of the FOV / FOV的角度范围
        public double Orientation { get; set; } // [deg] Central heading angle of the FOV FOV中轴线朝向角
        public double DistanceRange { get; set; } // [m] Detect range of the FOV / FOV探测距离范围
        public double BlindRange { get; set; } // [m] Blind range of the FOV / FOV盲区范围

        public FovDescription()
        {
            AngleRange = 90;
            DistanceRange = 100;
        }
    }

    class ObjectSensorSample : Sample
    {
        public List<ObjectInfo> Objects { get; set; } // List of objects / 目标物列表
        public int? KeyObjectFrontIndex { get; set; } // Index of CIPV object / 前向关键目标序号
        public int? KeyObjectLeftIndex { get; set; } // Index of left side key object / 左侧关键目标序号
        public int? KeyObjectRightIndex { get; set; } // Index of right side key object / 右侧关键目标序号

        public double? VehicleSpeed { get; set; } // [KPH] Subject vehicle's speed / 本车车速
        public double? VehicleCurvature { get; set; } // [1/m] Subject vehicle's turning curvature / 本车曲率
        public double? VehicleWidth { get; set; } // [m] Subject vehicle's width / 本车宽度
        public double? VehicleLength { get; set; } // [m] Subject vehicle's length / 本车长度
        public double? VehicleWheelBase { get; set; } // [m] Subject vehicle's wheel base / 本车轴距
        public double? VehicleFrontOverhang { get; set; } // [m] Subject vehicle's front overhang / 本车前悬

        public List<FovDescription> Fovs { get; set; } // List of FOV / FOV列表

        public ObjectSensorSample()
        {
            Objects = new List<ObjectInfo>();
            Fovs = new List<FovDescription>();
        }

        public static string Title
        {
            get
            {
                return "Object count,FOV count,Trajectory point count,Contour point count,Key object index (CIPV),Key object index (left-side),Key object index (right-side),Vehicle speed[KPH],Vehicle curvature[1/m],Vehicle width[m],Vehicle length[m],Vehicle wheel base[m],Vehicle front overhang[m],Reserved1,Reserved2,Reserved3,Reserved4,Reserved5,Reserved6,Reserved7,First object's ID / FOV's position-x[m],Object's age / FOV's position-y[m],Object's raw ID / FOV's orientation[°],Object's raw age / FOV's angle range[°],Object's raw class ID / FOV's distance range[m],Object's class / FOV's blind range[m],Object's position mode,Object's position-x[m],Object's position-y[m],Object position-z[m],Object's position-x sigma[m],Object's position-y sigma[m],Object's position-z sigma[m],Object's closest point-x[m],Object's closest point-y[m],Object's closest point distance[m],Object's width[m],Object's length[m],Object's height[m],Object's heading[°],Object's relative velocity-x[KPH],Object's absolute velocity-x[KPH],Object's relative velocity-y[KPH],Object's absolute velocity-y[KPH],Object's relative velocity-z[KPH],Object's absolute velocity-z[KPH],Object's relative acceleration-x[m/s2],Object's absolute acceleration-x[m/s2],Object's relative acceleration-y[m/s2],Object's absolute acceleration-y[m/s2],Object's relative acceleration-z[m/s2],Object's absolute acceleration-z[m/s2],Object's color(R-component),Object's color(G-component),Object's color(B-component),Object's time offset[us],Object's confidence[%],Object class's confidence[%],Object's TTC-1st[s],Object's TTC-2nd[s],Object's time headway[s],Object's potential DCA[m/s2],Object's overt DCA[m/s2],Object's overlap[%],Object's lateral clearance[m],Object's rear clearance[m],Object's RCS[dB],Object's polar longitudinal velocity[m/s],Object's polar lateral velocity[m/s],Object's yaw rate[°/s],Object's curvature[1/m],Object's speed[KPH],Object's reserved1,Object's reserved2,Object's reserved3,Object's reserved4,Object's reserved5,Object's reserved6,Object's reserved7,Object's reserved8,Object's reserved9,Object's reserved10,Object's reserved11,Object's reserved12,Object's reserved13,Object's reserved14,Object's reserved15,Object's reserved16,Object's trajectory data offset,Object's trajectory point count,Object's contour data offset,Object's contour point count,Second object's ID / FOV's position-x[m],etc.";
            }
        }

        public static string Protocol
        {
            get
            {
                return "obj-sensor-sample-v7";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new string[] { "obj-sensor-sample-v3", "obj-sensor-sample-v4", "obj-sensor-sample-v5", "obj-sensor-sample-v6", "obj-sensor-sample-v7" };
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample(int channel)
        {
            var sample = new GeneralSample();
            sample.SetTime(this);
            sample.Protocol = Protocol;
            sample.Channel = channel;

            int trajectoryPoints = 0;
            int contourPoints = 0;
            var trajectoryOffset = new int[Objects.Count];
            var trajectorySize = new int[Objects.Count];
            var contourOffset = new int[Objects.Count];
            var contourSize = new int[Objects.Count];
            for (int i = 0; i < Objects.Count; i++)
            {
                trajectoryOffset[i] = trajectoryPoints;
                trajectorySize[i] = Objects[i].Trajectory == null ? 0 : Objects[i].Trajectory.Length;
                trajectoryPoints += trajectorySize[i];
                contourOffset[i] = contourPoints;
                contourSize[i] = Objects[i].Contour == null ? 0 : Objects[i].Contour.Length;
                contourPoints += contourSize[i];
            }

            var data = new GeneralSampleValue[20 + Objects.Count * 72 + Fovs.Count * 6 + (trajectoryPoints + contourPoints) * 2];
            int objBase = 20;
            int fovBase = objBase + Objects.Count * 72;
            int trajectoryBase = fovBase + Fovs.Count * 6;
            int contourBase = trajectoryBase + trajectoryPoints * 2;

            data[0] = new GeneralSampleValue((double)Objects.Count);
            data[1] = new GeneralSampleValue((double)Fovs.Count);
            data[2] = new GeneralSampleValue((double)trajectoryPoints);
            data[3] = new GeneralSampleValue((double)contourPoints);

            if (KeyObjectFrontIndex != null) data[4] = new GeneralSampleValue((double)KeyObjectFrontIndex.Value);
            if (KeyObjectLeftIndex != null) data[5] = new GeneralSampleValue((double)KeyObjectLeftIndex.Value);
            if (KeyObjectRightIndex != null) data[6] = new GeneralSampleValue((double)KeyObjectRightIndex.Value);

            if (VehicleSpeed != null) data[7] = new GeneralSampleValue(VehicleSpeed.Value);
            if (VehicleCurvature != null) data[8] = new GeneralSampleValue(VehicleCurvature.Value);
            if (VehicleWidth != null) data[9] = new GeneralSampleValue(VehicleWidth.Value);
            if (VehicleLength != null) data[10] = new GeneralSampleValue(VehicleLength.Value);
            if (VehicleWheelBase != null) data[11] = new GeneralSampleValue(VehicleWheelBase.Value);
            if (VehicleFrontOverhang != null) data[12] = new GeneralSampleValue(VehicleFrontOverhang.Value);

            for (int i = 0; i < Objects.Count; i++)
            {
                ObjectInfo src = Objects[i];
                int baseIndex = objBase + 72 * i;
                data[baseIndex + 0] = new GeneralSampleValue((double)src.ID);
                data[baseIndex + 1] = new GeneralSampleValue((double)src.Age);
                if (src.RawID != null) data[baseIndex + 2] = new GeneralSampleValue((double)src.RawID.Value);
                if (src.RawAge != null) data[baseIndex + 3] = new GeneralSampleValue((double)src.RawAge.Value);
                if (src.ObjectRawClassID != null) data[baseIndex + 4] = new GeneralSampleValue((double)src.ObjectRawClassID.Value);
                data[baseIndex + 5] = new GeneralSampleValue((double)(uint)src.ObjectClass);
                data[baseIndex + 6] = new GeneralSampleValue((double)(uint)src.PosMode);
                data[baseIndex + 7] = new GeneralSampleValue(src.PosX);
                data[baseIndex + 8] = new GeneralSampleValue(src.PosY);
                if (src.PosZ != null) data[baseIndex + 9] = new GeneralSampleValue(src.PosZ.Value);
                if (src.PosXSigma != null) data[baseIndex + 10] = new GeneralSampleValue(src.PosXSigma.Value);
                if (src.PosYSigma != null) data[baseIndex + 11] = new GeneralSampleValue(src.PosYSigma.Value);
                if (src.PosZSigma != null) data[baseIndex + 12] = new GeneralSampleValue(src.PosZSigma.Value);
                data[baseIndex + 13] = new GeneralSampleValue(src.CPX);
                data[baseIndex + 14] = new GeneralSampleValue(src.CPY);
                data[baseIndex + 15] = new GeneralSampleValue(src.CPD);
                if (src.Width != null) data[baseIndex + 16] = new GeneralSampleValue(src.Width.Value);
                if (src.Length != null) data[baseIndex + 17] = new GeneralSampleValue(src.Length.Value);
                if (src.Height != null) data[baseIndex + 18] = new GeneralSampleValue(src.Height.Value);
                if (src.Heading != null) data[baseIndex + 19] = new GeneralSampleValue(src.Heading.Value);
                if (src.VXRel != null) data[baseIndex + 20] = new GeneralSampleValue(src.VXRel.Value);
                if (src.VXAbs != null) data[baseIndex + 21] = new GeneralSampleValue(src.VXAbs.Value);
                if (src.VYRel != null) data[baseIndex + 22] = new GeneralSampleValue(src.VYRel.Value);
                if (src.VYAbs != null) data[baseIndex + 23] = new GeneralSampleValue(src.VYAbs.Value);
                if (src.VZRel != null) data[baseIndex + 24] = new GeneralSampleValue(src.VZRel.Value);
                if (src.VZAbs != null) data[baseIndex + 25] = new GeneralSampleValue(src.VZAbs.Value);
                if (src.AXRel != null) data[baseIndex + 26] = new GeneralSampleValue(src.AXRel.Value);
                if (src.AXAbs != null) data[baseIndex + 27] = new GeneralSampleValue(src.AXAbs.Value);
                if (src.AYRel != null) data[baseIndex + 28] = new GeneralSampleValue(src.AYRel.Value);
                if (src.AYAbs != null) data[baseIndex + 29] = new GeneralSampleValue(src.AYAbs.Value);
                if (src.AZRel != null) data[baseIndex + 30] = new GeneralSampleValue(src.AZRel.Value);
                if (src.AZAbs != null) data[baseIndex + 31] = new GeneralSampleValue(src.AZAbs.Value);
                if (src.Color != null) data[baseIndex + 32] = new GeneralSampleValue(src.Color.Value.R);
                if (src.Color != null) data[baseIndex + 33] = new GeneralSampleValue(src.Color.Value.G);
                if (src.Color != null) data[baseIndex + 34] = new GeneralSampleValue(src.Color.Value.B);
                if (src.TimeOffset != null) data[baseIndex + 35] = new GeneralSampleValue(src.TimeOffset.Value);
                if (src.Confidence != null) data[baseIndex + 36] = new GeneralSampleValue(src.Confidence.Value);
                if (src.ObjectClassConfidence != null) data[baseIndex + 37] = new GeneralSampleValue(src.ObjectClassConfidence.Value);
                if (src.RCS != null) data[baseIndex + 46] = new GeneralSampleValue(src.RCS.Value);
                if (src.VLng != null) data[baseIndex + 47] = new GeneralSampleValue(src.VLng.Value);
                if (src.VLat != null) data[baseIndex + 48] = new GeneralSampleValue(src.VLat.Value);
                if (src.YawRate != null) data[baseIndex + 49] = new GeneralSampleValue(src.YawRate.Value);
                if (src.Curvature != null) data[baseIndex + 50] = new GeneralSampleValue(src.Curvature.Value);

                var ttc1 = GetTTC1(src);
                if (ttc1 != null) data[baseIndex + 38] = new GeneralSampleValue(ttc1.Value);
                var ttc2 = GetTTC2(src);
                if (ttc2 != null) data[baseIndex + 39] = new GeneralSampleValue(ttc2.Value);
                var thw = GetTHW(src);
                if (thw != null) data[baseIndex + 40] = new GeneralSampleValue(thw.Value);
                var pdca = GetDCA(src, 1.0);
                if (pdca != null) data[baseIndex + 41] = new GeneralSampleValue(pdca.Value);
                var odca = GetDCA(src, 0.0);
                if (odca != null) data[baseIndex + 42] = new GeneralSampleValue(odca.Value);
                var overlap = GetOverlap(src);
                if (overlap != null) data[baseIndex + 43] = new GeneralSampleValue(overlap.Value);
                var lc = GetLC(src);
                if (lc != null) data[baseIndex + 44] = new GeneralSampleValue(lc.Value);
                var rc = GetRC(src);
                if (rc != null) data[baseIndex + 45] = new GeneralSampleValue(rc.Value);

                var speed = src.Speed;
                if (speed != null) data[baseIndex + 51] = new GeneralSampleValue(speed.Value);

                data[baseIndex + 68] = new GeneralSampleValue((double)trajectoryOffset[i]);
                data[baseIndex + 69] = new GeneralSampleValue((double)trajectorySize[i]);
                data[baseIndex + 70] = new GeneralSampleValue((double)contourOffset[i]);
                data[baseIndex + 71] = new GeneralSampleValue((double)contourSize[i]);

                int dataOffset = trajectoryBase + trajectoryOffset[i] * 2;
                for (int n = 0; n < trajectorySize[i]; n++)
                {
                    var pt = src.Trajectory[n];
                    data[dataOffset + 2 * n] = new GeneralSampleValue(pt.X);
                    data[dataOffset + 2 * n + 1] = new GeneralSampleValue(pt.Y);
                }

                dataOffset = contourBase + contourOffset[i] * 2;
                for (int n = 0; n < contourSize[i]; n++)
                {
                    var pt = src.Contour[n];
                    data[dataOffset + 2 * n] = new GeneralSampleValue(pt.X);
                    data[dataOffset + 2 * n + 1] = new GeneralSampleValue(pt.Y);
                }
            }

            for (int i = 0; i < Fovs.Count; i++)
            {
                FovDescription src = Fovs[i];
                int baseIndex = fovBase + 6 * i;
                data[baseIndex + 0] = new GeneralSampleValue((double)src.PositionX);
                data[baseIndex + 1] = new GeneralSampleValue((double)src.PositionY);
                data[baseIndex + 2] = new GeneralSampleValue((double)src.Orientation);
                data[baseIndex + 3] = new GeneralSampleValue((double)src.AngleRange);
                data[baseIndex + 4] = new GeneralSampleValue((double)src.DistanceRange);
                data[baseIndex + 5] = new GeneralSampleValue((double)src.BlindRange);
            }

            sample.Values = data.ToList();
            sample.NumberOfSignificants = trajectoryBase;

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            SetTime(sample);

            #region obj-sensor-sample-v7
            if (sample.Protocol == "obj-sensor-sample-v7")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 20) return false;

                if (v[0].IsNotNumber() ||
                    v[1].IsNotNumber() ||
                    v[2].IsNotNumber() ||
                    v[3].IsNotNumber()) return false;

                int nObjects = (int)v[0].number;
                int nFovs = (int)v[1].number;
                int nTrajectories = (int)v[2].number;
                int nContours = (int)v[3].number;

                int sizeWithExtra = 20 + nObjects * 72 + nFovs * 6 + (nTrajectories + nContours) * 2;
                int sizeWithoutExtra = 20 + nObjects * 72 + nFovs * 6;
                if (v.Length != sizeWithExtra && v.Length != sizeWithoutExtra) return false;

                KeyObjectFrontIndex = v[4].ToInt();
                KeyObjectLeftIndex = v[5].ToInt();
                KeyObjectRightIndex = v[6].ToInt();

                VehicleSpeed = v[7].ToDouble();
                VehicleCurvature = v[8].ToDouble();
                VehicleWidth = v[9].ToDouble();
                VehicleLength = v[10].ToDouble();
                VehicleWheelBase = v[11].ToDouble();
                VehicleFrontOverhang = v[12].ToDouble();

                int objBase = 20;
                int fovBase = objBase + 72 * nObjects;
                int trajBase = fovBase + 6 * nFovs;
                int contBase = trajBase + 2 * nTrajectories;

                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    int baseIndex = objBase + 72 * i;
                    obj.ID = (int)v[baseIndex + 0].number;
                    obj.Age = (int)v[baseIndex + 1].number;
                    obj.RawID = v[baseIndex + 2].ToInt();
                    obj.RawAge = v[baseIndex + 3].ToInt();
                    obj.ObjectRawClassID = v[baseIndex + 4].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[baseIndex + 5].number;
                    obj.PosMode = (PositionMode)(int)v[baseIndex + 6].number;
                    obj.PosX = v[baseIndex + 7].number;
                    obj.PosY = v[baseIndex + 8].number;
                    obj.PosZ = v[baseIndex + 9].ToDouble();
                    obj.PosXSigma = v[baseIndex + 10].ToDouble();
                    obj.PosYSigma = v[baseIndex + 11].ToDouble();
                    obj.PosZSigma = v[baseIndex + 12].ToDouble();
                    obj.CPX = v[baseIndex + 13].number;
                    obj.CPY = v[baseIndex + 14].number;
                    obj.CPD = v[baseIndex + 15].number;
                    obj.Width = v[baseIndex + 16].ToDouble();
                    obj.Length = v[baseIndex + 17].ToDouble();
                    obj.Height = v[baseIndex + 18].ToDouble();
                    obj.Heading = v[baseIndex + 19].ToDouble();
                    obj.VXRel = v[baseIndex + 20].ToDouble();
                    obj.VXAbs = v[baseIndex + 21].ToDouble();
                    obj.VYRel = v[baseIndex + 22].ToDouble();
                    obj.VYAbs = v[baseIndex + 23].ToDouble();
                    obj.VZRel = v[baseIndex + 24].ToDouble();
                    obj.VZAbs = v[baseIndex + 25].ToDouble();
                    obj.AXRel = v[baseIndex + 26].ToDouble();
                    obj.AXAbs = v[baseIndex + 27].ToDouble();
                    obj.AYRel = v[baseIndex + 28].ToDouble();
                    obj.AYAbs = v[baseIndex + 29].ToDouble();
                    obj.AZRel = v[baseIndex + 30].ToDouble();
                    obj.AZAbs = v[baseIndex + 31].ToDouble();
                    if (v[baseIndex + 32].IsNumber()) obj.Color = new ColorRGBA((byte)v[baseIndex + 32].number, (byte)v[baseIndex + 33].number, (byte)v[baseIndex + 34].number);
                    obj.TimeOffset = v[baseIndex + 35].ToInt();
                    obj.Confidence = v[baseIndex + 36].ToDouble();
                    obj.ObjectClassConfidence = v[baseIndex + 37].ToDouble();
                    obj.RCS = v[baseIndex + 46].ToDouble();
                    obj.VLng = v[baseIndex + 47].ToDouble();
                    obj.VLat = v[baseIndex + 48].ToDouble();
                    obj.YawRate = v[baseIndex + 49].ToDouble();
                    obj.Curvature = v[baseIndex + 50].ToDouble();

                    int trajOffset = (int)v[baseIndex + 68].number;
                    int trajSize = (int)v[baseIndex + 69].number;
                    int contOffset = (int)v[baseIndex + 70].number;
                    int contSize = (int)v[baseIndex + 71].number;

                    if (v.Length == sizeWithExtra)
                    {
                        obj.Trajectory = new FloatPoint[trajSize];
                        for (int n = 0; n < trajSize; n++)
                        {
                            obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                            obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                        }

                        obj.Contour = new FloatPoint[contSize];
                        for (int n = 0; n < contSize; n++)
                        {
                            obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                            obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                        }
                    }
                    else
                    {
                        obj.Trajectory = new FloatPoint[0];
                        obj.Contour = new FloatPoint[0];
                    }

                    Objects.Add(obj);
                }

                Fovs.Clear();
                for (int i = 0; i < nFovs; i++)
                {
                    var fov = new FovDescription();
                    int baseIndex = fovBase + 6 * i;

                    fov.PositionX = v[baseIndex].number;
                    fov.PositionY = v[baseIndex + 1].number;
                    fov.Orientation = v[baseIndex + 2].number;
                    fov.AngleRange = v[baseIndex + 3].number;
                    fov.DistanceRange = v[baseIndex + 4].number;
                    fov.BlindRange = v[baseIndex + 5].number;

                    Fovs.Add(fov);
                }

                return true;
            }
            #endregion

            #region obj-sensor-sample-v6
            else if (sample.Protocol == "obj-sensor-sample-v6")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 16) return false;

                if (v[0].IsNotNumber() ||
                    v[1].IsNotNumber() ||
                    v[2].IsNotNumber() ||
                    v[3].IsNotNumber()) return false;

                int nObjects = (int)v[0].number;
                int nFovs = (int)v[1].number;
                int nTrajectories = (int)v[2].number;
                int nContours = (int)v[3].number;

                int sizeWithExtra = 16 + nObjects * 52 + nFovs * 6 + (nTrajectories + nContours) * 2;
                int sizeWithoutExtra = 16 + nObjects * 52 + nFovs * 6;
                if (v.Length != sizeWithExtra && v.Length != sizeWithoutExtra) return false;

                KeyObjectFrontIndex = v[4].ToInt();
                KeyObjectLeftIndex = v[5].ToInt();
                KeyObjectRightIndex = v[6].ToInt();

                VehicleSpeed = v[7].ToDouble();
                VehicleCurvature = v[8].ToDouble();
                VehicleWidth = v[9].ToDouble();
                VehicleLength = v[10].ToDouble();
                VehicleWheelBase = v[11].ToDouble();
                VehicleFrontOverhang = v[12].ToDouble();

                int objBase = 16;
                int fovBase = objBase + 52 * nObjects;
                int trajBase = fovBase + 6 * nFovs;
                int contBase = trajBase + 2 * nTrajectories;

                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    int baseIndex = objBase + 52 * i;
                    obj.ID = (int)v[baseIndex + 0].number;
                    obj.Age = (int)v[baseIndex + 1].number;
                    obj.RawID = v[baseIndex + 2].ToInt();
                    obj.RawAge = v[baseIndex + 3].ToInt();
                    obj.ObjectRawClassID = v[baseIndex + 4].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[baseIndex + 5].number;
                    obj.PosMode = (PositionMode)(int)v[baseIndex + 6].number;
                    obj.PosX = v[baseIndex + 7].number;
                    obj.PosY = v[baseIndex + 8].number;
                    obj.CPX = v[baseIndex + 9].number;
                    obj.CPY = v[baseIndex + 10].number;
                    obj.CPD = v[baseIndex + 11].number;
                    obj.Width = v[baseIndex + 12].ToDouble();
                    obj.Length = v[baseIndex + 13].ToDouble();
                    obj.Heading = v[baseIndex + 14].ToDouble();
                    obj.VXRel = v[baseIndex + 15].ToDouble();
                    obj.VXAbs = v[baseIndex + 16].ToDouble();
                    obj.VYRel = v[baseIndex + 17].ToDouble();
                    obj.VYAbs = v[baseIndex + 18].ToDouble();
                    obj.AXRel = v[baseIndex + 19].ToDouble();
                    obj.AXAbs = v[baseIndex + 20].ToDouble();
                    obj.AYRel = v[baseIndex + 21].ToDouble();
                    obj.AYAbs = v[baseIndex + 22].ToDouble();
                    if (v[baseIndex + 23].IsNumber()) obj.Color = new ColorRGBA((byte)v[baseIndex + 23].number, (byte)v[baseIndex + 24].number, (byte)v[baseIndex + 25].number);
                    obj.TimeOffset = v[baseIndex + 26].ToInt();
                    obj.Confidence = v[baseIndex + 27].ToDouble();
                    obj.ObjectClassConfidence = v[baseIndex + 28].ToDouble();
                    obj.Height = v[baseIndex + 29].ToDouble();
                    obj.PosZ = v[baseIndex + 30].ToDouble();
                    obj.PosXSigma = v[baseIndex + 31].ToDouble();
                    obj.PosYSigma = v[baseIndex + 32].ToDouble();
                    obj.PosZSigma = v[baseIndex + 33].ToDouble();
                    obj.RCS = v[baseIndex + 42].ToDouble();
                    obj.VLng = v[baseIndex + 43].ToDouble();
                    obj.VLat = v[baseIndex + 44].ToDouble();
                    obj.YawRate = v[baseIndex + 45].ToDouble();
                    obj.Curvature = v[baseIndex + 46].ToDouble();

                    int trajOffset = (int)v[baseIndex + 48].number;
                    int trajSize = (int)v[baseIndex + 49].number;
                    int contOffset = (int)v[baseIndex + 50].number;
                    int contSize = (int)v[baseIndex + 51].number;

                    if (v.Length == sizeWithExtra)
                    {
                        obj.Trajectory = new FloatPoint[trajSize];
                        for (int n = 0; n < trajSize; n++)
                        {
                            obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                            obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                        }

                        obj.Contour = new FloatPoint[contSize];
                        for (int n = 0; n < contSize; n++)
                        {
                            obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                            obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                        }
                    }
                    else
                    {
                        obj.Trajectory = new FloatPoint[0];
                        obj.Contour = new FloatPoint[0];
                    }

                    Objects.Add(obj);
                }

                Fovs.Clear();
                for (int i = 0; i < nFovs; i++)
                {
                    var fov = new FovDescription();
                    int baseIndex = fovBase + 6 * i;

                    fov.PositionX = v[baseIndex].number;
                    fov.PositionY = v[baseIndex + 1].number;
                    fov.Orientation = v[baseIndex + 2].number;
                    fov.AngleRange = v[baseIndex + 3].number;
                    fov.DistanceRange = v[baseIndex + 4].number;
                    fov.BlindRange = v[baseIndex + 5].number;

                    Fovs.Add(fov);
                }

                return true;
            }
            #endregion

            #region obj-sensor-sample-v5
            else if (sample.Protocol == "obj-sensor-sample-v5")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 11) return false;

                if (v[0].IsNotNumber() ||
                    v[1].IsNotNumber() ||
                    v[2].IsNotNumber()) return false;

                int nObjects = (int)v[0].number;
                int nTrajectories = (int)v[1].number;
                int nContours = (int)v[2].number;
                int nFovs = (int)v[10].number;

                int sizeWithExtra = 11 + nObjects * 42 + nFovs * 6 + (nTrajectories + nContours) * 2;
                int sizeWithoutExtra = 11 + nObjects * 42 + nFovs * 6;
                if (v.Length != sizeWithExtra && v.Length != sizeWithoutExtra) return false;

                KeyObjectFrontIndex = v[3].ToInt();
                KeyObjectLeftIndex = v[4].ToInt();
                KeyObjectRightIndex = v[5].ToInt();

                VehicleSpeed = v[6].ToDouble();
                VehicleCurvature = v[7].ToDouble();
                VehicleWidth = v[8].ToDouble();
                VehicleLength = v[9].ToDouble();

                int objBase = 11;
                int fovBase = objBase + 42 * nObjects;
                int trajBase = fovBase + 6 * nFovs;
                int contBase = trajBase + 2 * nTrajectories;

                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    int baseIndex = objBase + 42 * i;
                    obj.ID = (int)v[baseIndex + 0].number;
                    obj.Age = (int)v[baseIndex + 1].number;
                    obj.RawID = v[baseIndex + 2].ToInt();
                    obj.RawAge = v[baseIndex + 3].ToInt();
                    obj.ObjectRawClassID = v[baseIndex + 4].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[baseIndex + 5].number;
                    obj.PosMode = (PositionMode)(int)v[baseIndex + 6].number;
                    obj.PosX = v[baseIndex + 7].number;
                    obj.PosY = v[baseIndex + 8].number;
                    obj.CPX = v[baseIndex + 9].number;
                    obj.CPY = v[baseIndex + 10].number;
                    obj.CPD = v[baseIndex + 11].number;
                    obj.Width = v[baseIndex + 12].ToDouble();
                    obj.Length = v[baseIndex + 13].ToDouble();
                    obj.Heading = v[baseIndex + 14].ToDouble();
                    obj.VXRel = v[baseIndex + 15].ToDouble();
                    obj.VXAbs = v[baseIndex + 16].ToDouble();
                    obj.VYRel = v[baseIndex + 17].ToDouble();
                    obj.VYAbs = v[baseIndex + 18].ToDouble();
                    obj.AXRel = v[baseIndex + 19].ToDouble();
                    obj.AXAbs = v[baseIndex + 20].ToDouble();
                    obj.AYRel = v[baseIndex + 21].ToDouble();
                    obj.AYAbs = v[baseIndex + 22].ToDouble();
                    obj.TimeOffset = v[baseIndex + 35].ToInt();
                    if (v[baseIndex + 36].IsNumber()) obj.Color = new ColorRGBA((byte)v[baseIndex + 36].number, (byte)v[baseIndex + 37].number, (byte)v[baseIndex + 38].number);
                    obj.ObjectClassConfidence = v[baseIndex + 39].ToDouble();
                    obj.PosXSigma = v[baseIndex + 40].ToDouble();
                    obj.PosYSigma = v[baseIndex + 41].ToDouble();

                    int trajOffset = (int)v[baseIndex + 31].number;
                    int trajSize = (int)v[baseIndex + 32].number;
                    int contOffset = (int)v[baseIndex + 33].number;
                    int contSize = (int)v[baseIndex + 34].number;

                    if (v.Length == sizeWithExtra)
                    {
                        obj.Trajectory = new FloatPoint[trajSize];
                        for (int n = 0; n < trajSize; n++)
                        {
                            obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                            obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                        }

                        obj.Contour = new FloatPoint[contSize];
                        for (int n = 0; n < contSize; n++)
                        {
                            obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                            obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                        }
                    }
                    else
                    {
                        obj.Trajectory = new FloatPoint[0];
                        obj.Contour = new FloatPoint[0];
                    }

                    Objects.Add(obj);
                }

                Fovs.Clear();
                for (int i = 0; i < nFovs; i++)
                {
                    var fov = new FovDescription();
                    int baseIndex = fovBase + 6 * i;

                    fov.PositionX = v[baseIndex].number;
                    fov.PositionY = v[baseIndex + 1].number;
                    fov.Orientation = v[baseIndex + 2].number;
                    fov.AngleRange = v[baseIndex + 3].number;
                    fov.DistanceRange = v[baseIndex + 4].number;
                    fov.BlindRange = v[baseIndex + 5].number;

                    Fovs.Add(fov);
                }

                return true;
            }
            #endregion

            #region obj-sensor-sample-v4
            else if (sample.Protocol == "obj-sensor-sample-v4")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 16) return false;

                if (v[0].IsNotNumber() ||
                    v[1].IsNotNumber() ||
                    v[2].IsNotNumber()) return false;

                int nObjects = (int)v[0].number;
                int nTrajectories = (int)v[1].number;
                int nContours = (int)v[2].number;
                if (v.Length != 16 + nObjects * 35 + (nTrajectories + nContours) * 2) return false;

                KeyObjectFrontIndex = v[3].ToInt();
                KeyObjectLeftIndex = v[4].ToInt();
                KeyObjectRightIndex = v[5].ToInt();

                VehicleSpeed = v[6].ToDouble();
                VehicleCurvature = v[7].ToDouble();
                VehicleWidth = v[8].ToDouble();
                VehicleLength = v[9].ToDouble();

                double fovPosX = v[10].number;
                double fovPosY = v[11].number;
                double fovOrient = v[12].number;
                double fovAngleRange = v[13].number;
                double fovDistRange = v[14].number;
                bool fovSymmetry = v[15].number == 1;

                Fovs.Clear();
                Fovs.Add(new FovDescription()
                    {
                        PositionX = fovPosX,
                        PositionY = fovPosY,
                        Orientation = fovOrient,
                        AngleRange = fovAngleRange,
                        DistanceRange = fovDistRange,
                    });
                if (fovSymmetry)
                {
                    Fovs.Add(new FovDescription()
                        {
                            PositionX = fovPosX,
                            PositionY = -fovPosY,
                            Orientation = -fovOrient,
                            AngleRange = fovAngleRange,
                            DistanceRange = fovDistRange,
                        });
                }

                int trajBase = 16 + 35 * nObjects;
                int contBase = 16 + 35 * nObjects + 2 * nTrajectories;
                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    int baseIndex = 16 + 35 * i;
                    obj.ID = (int)v[baseIndex + 0].number;
                    obj.Age = (int)v[baseIndex + 1].number;
                    obj.RawID = v[baseIndex + 2].ToInt();
                    obj.RawAge = v[baseIndex + 3].ToInt();
                    obj.ObjectRawClassID = v[baseIndex + 4].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[baseIndex + 5].number;
                    obj.PosMode = (PositionMode)(int)v[baseIndex + 6].number;
                    obj.PosX = v[baseIndex + 7].number;
                    obj.PosY = v[baseIndex + 8].number;
                    obj.CPX = v[baseIndex + 9].number;
                    obj.CPY = v[baseIndex + 10].number;
                    obj.CPD = v[baseIndex + 11].number;
                    obj.Width = v[baseIndex + 12].ToDouble();
                    obj.Length = v[baseIndex + 13].ToDouble();
                    obj.Heading = v[baseIndex + 14].ToDouble();
                    obj.VXRel = v[baseIndex + 15].ToDouble();
                    obj.VXAbs = v[baseIndex + 16].ToDouble();
                    obj.VYRel = v[baseIndex + 17].ToDouble();
                    obj.VYAbs = v[baseIndex + 18].ToDouble();
                    obj.AXRel = v[baseIndex + 19].ToDouble();
                    obj.AXAbs = v[baseIndex + 20].ToDouble();
                    obj.AYRel = v[baseIndex + 21].ToDouble();
                    obj.AYAbs = v[baseIndex + 22].ToDouble();

                    int trajOffset = (int)v[baseIndex + 31].number;
                    int trajSize = (int)v[baseIndex + 32].number;
                    int contOffset = (int)v[baseIndex + 33].number;
                    int contSize = (int)v[baseIndex + 34].number;

                    obj.Trajectory = new FloatPoint[trajSize];
                    for (int n = 0; n < trajSize; n++)
                    {
                        obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                        obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                    }

                    obj.Contour = new FloatPoint[contSize];
                    for (int n = 0; n < contSize; n++)
                    {
                        obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                        obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                    }

                    Objects.Add(obj);
                }

                return true;
            }
            #endregion

            #region obj-sensor-sample-v3
            else if (sample.Protocol == "obj-sensor-sample-v3")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 7) return false;
                if (v[0].IsNotNumber() || v[0].number != sample.Channel.Value) return false;

                if (v[1].IsNotNumber() ||
                    v[2].IsNotNumber() ||
                    v[3].IsNotNumber()) return false;

                int nObjects = (int)v[1].number;
                int nTrajectories = (int)v[2].number;
                int nContours = (int)v[3].number;
                if (v.Length != 7 + nObjects * 33 + (nTrajectories + nContours) * 2) return false;

                KeyObjectFrontIndex = v[4].ToInt();
                KeyObjectLeftIndex = v[5].ToInt();
                KeyObjectRightIndex = v[6].ToInt();

                int trajBase = 7 + 33 * nObjects;
                int contBase = 7 + 33 * nObjects + 2 * nTrajectories;
                Objects.Clear();
                for (int i = 0; i < nObjects; i++)
                {
                    var obj = new ObjectInfo();
                    obj.ID = (int)v[33 * i + 7].number;
                    obj.Age = (int)v[33 * i + 8].number;
                    obj.ObjectRawClassID = v[33 * i + 9].ToInt();
                    obj.ObjectClass = (ObjectClass)(int)v[33 * i + 10].number;
                    obj.PosMode = (PositionMode)(int)v[33 * i + 11].number;
                    obj.PosX = v[33 * i + 12].number;
                    obj.PosY = v[33 * i + 13].number;
                    obj.CPX = v[33 * i + 14].number;
                    obj.CPY = v[33 * i + 15].number;
                    obj.Width = v[33 * i + 17].ToDouble();
                    obj.Length = v[33 * i + 18].ToDouble();
                    obj.Heading = v[33 * i + 19].ToDouble();
                    obj.VXRel = v[33 * i + 20].ToDouble();
                    obj.VXAbs = v[33 * i + 21].ToDouble();
                    obj.VYRel = v[33 * i + 22].ToDouble();
                    obj.VYAbs = v[33 * i + 23].ToDouble();
                    obj.AXRel = v[33 * i + 24].ToDouble();
                    obj.AXAbs = v[33 * i + 25].ToDouble();
                    obj.AYRel = v[33 * i + 26].ToDouble();
                    obj.AYAbs = v[33 * i + 27].ToDouble();

                    int trajOffset = (int)v[33 * i + 36].number;
                    int trajSize = (int)v[33 * i + 37].number;
                    int contOffset = (int)v[33 * i + 38].number;
                    int contSize = (int)v[33 * i + 39].number;

                    obj.Trajectory = new FloatPoint[trajSize];
                    for (int n = 0; n < trajSize; n++)
                    {
                        obj.Trajectory[n].X = (float)v[trajBase + 2 * trajOffset + 2 * n].number;
                        obj.Trajectory[n].Y = (float)v[trajBase + 2 * trajOffset + 2 * n + 1].number;
                    }

                    obj.Contour = new FloatPoint[contSize];
                    for (int n = 0; n < contSize; n++)
                    {
                        obj.Contour[n].X = (float)v[contBase + 2 * contOffset + 2 * n].number;
                        obj.Contour[n].Y = (float)v[contBase + 2 * contOffset + 2 * n + 1].number;
                    }

                    Objects.Add(obj);
                }

                return true;
            }
            #endregion

            return false;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        protected override Sample Interpolate(SearchResult input)
        {
            var s1 = input.s1 as ObjectSensorSample;
            var s2 = input.s2 as ObjectSensorSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new ObjectSensorSample();

            // Vehicle, FOV
            output.VehicleWidth = s1.VehicleWidth;
            output.VehicleLength = s1.VehicleLength;
            output.VehicleSpeed = s1.VehicleSpeed * w1 + s2.VehicleSpeed * w2;
            output.VehicleCurvature = s1.VehicleCurvature * w1 + s2.VehicleCurvature * w2;
            output.VehicleWheelBase = s1.VehicleWheelBase;
            output.VehicleFrontOverhang = s1.VehicleFrontOverhang;

            output.Fovs = s1.Fovs;

            int? koFrontID = null, koLeftID = null, koRightID = null;
            if (s1.KeyObjectFrontIndex != null && s2.KeyObjectFrontIndex != null &&
                s1.Objects[s1.KeyObjectFrontIndex.Value].ID == s2.Objects[s2.KeyObjectFrontIndex.Value].ID)
            {
                koFrontID = s1.Objects[s1.KeyObjectFrontIndex.Value].ID;
            }
            if (s1.KeyObjectLeftIndex != null && s2.KeyObjectLeftIndex != null &&
                s1.Objects[s1.KeyObjectLeftIndex.Value].ID == s2.Objects[s2.KeyObjectLeftIndex.Value].ID)
            {
                koLeftID = s1.Objects[s1.KeyObjectLeftIndex.Value].ID;
            }
            if (s1.KeyObjectRightIndex != null && s2.KeyObjectRightIndex != null &&
                s1.Objects[s1.KeyObjectRightIndex.Value].ID == s2.Objects[s2.KeyObjectRightIndex.Value].ID)
            {
                koRightID = s1.Objects[s1.KeyObjectRightIndex.Value].ID;
            }

            output.Objects = new List<ObjectInfo>();
            int index = 0;
            foreach (var o1 in s1.Objects)
            {
                var id = o1.ID;
                foreach (var o2 in s2.Objects)
                {
                    if (o2.ID != id) continue;

                    if (koFrontID != null && koFrontID.Value == id) output.KeyObjectFrontIndex = index;
                    if (koLeftID != null && koLeftID.Value == id) output.KeyObjectLeftIndex = index;
                    if (koRightID != null && koRightID.Value == id) output.KeyObjectRightIndex = index;

                    double ow1 = w1, ow2 = w2;
                    if (o1.TimeOffset != null || o2.TimeOffset != null)
                    {
                        var ot1 = s1.Timeline;
                        if (o1.TimeOffset != null) ot1 += 0.000001 * o1.TimeOffset.Value;
                        var ot2 = s2.Timeline;
                        if (o2.TimeOffset != null) ot2 += 0.000001 * o2.TimeOffset.Value;
                        if (ot2 - ot1 >= 0.000001)
                        {
                            var targetTime = s1.Timeline + (s2.Timeline - s1.Timeline) * w2;
                            ow1 = (ot2 - targetTime) / (ot2 - ot1);
                            ow2 = (targetTime - ot1) / (ot2 - ot1);
                        }
                    }

                    var newObj = new ObjectInfo();
                    newObj.ID = id;
                    newObj.Age = o1.Age;
                    newObj.RawID = o1.RawID;
                    newObj.RawAge = o1.RawAge;
                    newObj.Confidence = o1.Confidence * ow1 + o2.Confidence * ow2;
                    newObj.Color = ow1 > ow2 ? o1.Color : o2.Color;
                    newObj.ObjectClass = ow1 > ow2 ? o1.ObjectClass : o2.ObjectClass;
                    newObj.ObjectRawClassID = ow1 > ow2 ? o1.ObjectRawClassID : o2.ObjectRawClassID;
                    newObj.ObjectClassConfidence = o1.ObjectClassConfidence * ow1 + o2.ObjectClassConfidence * ow2;
                    newObj.PosMode = o1.PosMode;
                    newObj.PosX = o1.PosX * ow1 + o2.PosX * ow2;
                    newObj.PosY = o1.PosY * ow1 + o2.PosY * ow2;
                    newObj.PosZ = o1.PosZ * ow1 + o2.PosZ * ow2;
                    newObj.PosXSigma = o1.PosXSigma * ow1 + o2.PosXSigma * ow2;
                    newObj.PosYSigma = o1.PosYSigma * ow1 + o2.PosYSigma * ow2;
                    newObj.PosZSigma = o1.PosZSigma * ow1 + o2.PosZSigma * ow2;
                    newObj.CPX = o1.CPX * ow1 + o2.CPX * ow2;
                    newObj.CPY = o1.CPY * ow1 + o2.CPY * ow2;
                    newObj.CPD = o1.CPD * ow1 + o2.CPD * ow2;
                    newObj.VXAbs = o1.VXAbs * ow1 + o2.VXAbs * ow2;
                    newObj.VYAbs = o1.VYAbs * ow1 + o2.VYAbs * ow2;
                    newObj.VZAbs = o1.VZAbs * ow1 + o2.VZAbs * ow2;
                    newObj.AXAbs = o1.AXAbs * ow1 + o2.AXAbs * ow2;
                    newObj.AYAbs = o1.AYAbs * ow1 + o2.AYAbs * ow2;
                    newObj.AZAbs = o1.AZAbs * ow1 + o2.AZAbs * ow2;
                    newObj.VXRel = o1.VXRel * ow1 + o2.VXRel * ow2;
                    newObj.VYRel = o1.VYRel * ow1 + o2.VYRel * ow2;
                    newObj.VZRel = o1.VZRel * ow1 + o2.VZRel * ow2;
                    newObj.AXRel = o1.AXRel * ow1 + o2.AXRel * ow2;
                    newObj.AYRel = o1.AYRel * ow1 + o2.AYRel * ow2;
                    newObj.AZRel = o1.AZRel * ow1 + o2.AZRel * ow2;
                    newObj.RCS = o1.RCS * ow1 + o2.RCS * ow2;
                    newObj.VLng = o1.VLng * ow1 + o2.VLng * ow2;
                    newObj.VLat = o1.VLat * ow1 + o2.VLat * ow2;
                    newObj.YawRate = o1.YawRate * ow1 + o2.YawRate * ow2;
                    newObj.Curvature = o1.Curvature * ow1 + o2.Curvature * ow2;
                    newObj.Heading = interpolateAngle(o1.Heading, ow1, o2.Heading, ow2);
                    newObj.Width = o1.Width * ow1 + o2.Width * ow2;
                    newObj.Length = o1.Length * ow1 + o2.Length * ow2;
                    newObj.Height = o1.Height * ow1 + o2.Height * ow2;
                    newObj.Trajectory = ow1 > ow2 ? o1.Trajectory : o2.Trajectory;
                    newObj.Contour = ow1 > ow2 ? o1.Contour : o2.Contour;

                    output.Objects.Add(newObj);
                    index++;
                }
            }

            return output;
        }

        public double? GetTTC1(ObjectInfo obj) // Calculate first order TTC / 计算一阶TTC [s]
        {
            if (obj.VXRel != null)
            {
                double vxRelM = obj.VXRel.Value / 3.6;

                if (obj.CPX > 0)
                {
                    if (vxRelM >= -0.1) return 20;
                    else return Math.Min(20, -(obj.CPX / vxRelM));
                }
                else if (VehicleLength != null && obj.CPX < -VehicleLength.Value)
                {
                    if (vxRelM <= 0.1) return 20;
                    else return Math.Min(20, (-VehicleLength.Value - obj.CPX) / vxRelM);
                }
                else return null;
            }
            else return null;
        }

        public double? GetTTC2(ObjectInfo obj) // Calculate second order TTC / 计算二阶TTC [s]
        {
            if (obj.VXRel != null && obj.AXRel != null)
            {
                double vxRelM = obj.VXRel.Value / 3.6;
                double axRel = obj.AXRel.Value;

                if (obj.CPX > 0)
                {
                    if (vxRelM >= -0.1) return 20;
                    else if (Math.Abs(axRel) < 0.1) return Math.Min(20, -(obj.CPX / vxRelM));
                    else if (vxRelM * vxRelM - 2 * axRel * obj.CPX >= 0) return Math.Min(20, (-vxRelM - Math.Sqrt(vxRelM * vxRelM - 2 * axRel * obj.CPX)) / axRel); //0.5at^2 + vt - d = 0
                    else return 20;
                }
                else if (VehicleLength != null && obj.CPX < -VehicleLength.Value)
                {
                    double dist = -VehicleLength.Value - obj.CPX;

                    if (vxRelM <= 0.1) return 20;
                    else if (Math.Abs(axRel) < 0.1) return Math.Min(20, (-VehicleLength.Value - obj.CPX) / vxRelM);
                    else if (vxRelM * vxRelM + 2 * axRel * dist >= 0) return Math.Min(20, (-vxRelM + Math.Sqrt(vxRelM * vxRelM + 2 * axRel * dist)) / axRel);
                    return 20;
                }
                else return null;
            }
            else return null;
        }

        public double? GetTHW(ObjectInfo obj) // Calculate time headway / 计算车间时距 [s]
        {
            if (obj.CPX > 0 && VehicleSpeed != null)
            {
                double vxEgoM = VehicleSpeed.Value / 3.6;
                if (vxEgoM <= 0.1) return 20;
                else return Math.Min(20, obj.CPX / vxEgoM);
            }
            else return null;
        }

        public double? GetDCA(ObjectInfo obj, double reactionTime = 0) // Calculate DCA / 计算避免碰撞最小减速度 [m/s²] reactionTime=0: ODCA, >0: PDCA
        {
            if (obj.CPX > 0 && VehicleSpeed != null && obj.VXAbs != null && obj.AXAbs != null)
            {
                double vxEgoM = VehicleSpeed.Value / 3.6;
                double vxAbsM = obj.VXAbs.Value / 3.6;
                double axAbs = obj.AXAbs.Value;

                double egoDxReaction = vxEgoM * reactionTime;
                double objDxReaction = vxAbsM * reactionTime + 0.5 * axAbs * reactionTime * reactionTime;

                double dxRelAR = obj.CPX - 0.1 + objDxReaction - egoDxReaction;
                double egoVxAbsAR = vxEgoM;
                double objVxAbsAR = vxAbsM + axAbs * reactionTime;
                double objVxRelAR = vxAbsM - vxEgoM;
                double objAxAbsAR = axAbs;

                if (dxRelAR <= 0) return -20;

                if (objAxAbsAR >= 0)
                {
                    if (objVxRelAR >= 0) return 0;
                    else return Math.Max(-20, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
                }
                else
                {
                    if (objVxRelAR >= 0) return Math.Max(-20, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
                    if (objVxRelAR * objVxRelAR / (2 * dxRelAR) < objAxAbsAR) return Math.Max(-20, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
                    else return Math.Max(-20, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
                }
            }
            else return null;
        }

        public double? GetOverlap(ObjectInfo obj, double boxCornerRatio = 0.98) // Calculate overlap / 计算重叠率 [%]
        {
            if (VehicleCurvature != null && VehicleWidth != null)
            {
                double widthEgo = VehicleWidth.Value;
                double curvEgo = VehicleCurvature.Value;
                double fo = VehicleFrontOverhang == null ? 0.9 : VehicleFrontOverhang.Value;
                double wb = VehicleWheelBase == null ? 2.8 : VehicleWheelBase.Value;
                
                if (obj.PosX <= 0 || widthEgo <= 0) return null;

                double widthObj;
                if (obj.PosMode == PositionMode.ClosestPoint)
                {
                    switch (obj.ObjectCategory)
                    {
                        case ObjectClass.Car:
                            widthObj = 1.9;
                            break;
                        case ObjectClass.Pedestrian:
                            widthObj = 0.5;
                            break;
                        case ObjectClass.Truck:
                            widthObj = 2.1;
                            break;
                        case ObjectClass.Bike:
                            widthObj = 0.6;
                            break;
                        case ObjectClass.RailCar:
                            widthObj = 2.6;
                            break;
                        default:
                            widthObj = 1.0;
                            break;
                    }
                }
                else  //BoxCenter
                {
                    var corners = CalBoxCorners(obj, boxCornerRatio);
                    double miny = double.PositiveInfinity, maxy = double.NegativeInfinity;
                    for (int i = 0; i < corners.Length; i++)
                    {
                        miny = Math.Min(miny, corners[i].Y);
                        maxy = Math.Max(maxy, corners[i].Y);
                    }
                    widthObj = maxy - miny;
                }

                double dyEgo = 0.5 * curvEgo * obj.PosX * obj.PosX + curvEgo * (fo + wb) * obj.PosX;
                double dyObj = obj.PosY;
                double overLap = (Math.Min(dyEgo + 0.5 * widthEgo, dyObj + 0.5 * widthObj) - Math.Max(dyEgo - 0.5 * widthEgo, dyObj - 0.5 * widthObj));
                double overLapRate = overLap / (overLap >= 0 ? Math.Min(widthEgo, widthObj) : widthEgo);
                return (int)(Math.Max(-3.0, overLapRate) * 100);
            }
            else return null;
        }

        public double? GetLC(ObjectInfo obj, double boxCornerRatio = 0.98) // Calculate lateral clearance / 计算横向间距 [m]
        {
            if (VehicleWidth != null)
            {
                if (obj.PosMode == PositionMode.ClosestPoint)
                {
                    if (obj.PosX > 0) return null;
                    else if (obj.PosY > 0) return obj.PosY - VehicleWidth.Value * 0.5;
                    else return -VehicleWidth.Value * 0.5 - obj.PosY;
                }
                else // BoxCenter
                {
                    var points = CalBoxCorners(obj, boxCornerRatio);

                    double minx = double.PositiveInfinity;
                    double miny = double.PositiveInfinity, maxy = double.NegativeInfinity;
                    for (int i = 0; i < points.Length; i++)
                    {
                        var pt = points[i];
                        if (pt.X < minx) minx = pt.X;
                        if (pt.Y < miny) miny = pt.Y;
                        if (pt.Y > maxy) maxy = pt.Y;
                    }

                    if (minx > 0) return null;
                    else if (miny > 0) return miny - VehicleWidth.Value * 0.5;
                    else if (maxy < 0) return -VehicleWidth.Value * 0.5 - maxy;
                    else return null;
                }
            }
            else return null;
        }

        public double? GetRC(ObjectInfo obj, double boxCornerRatio = 0.98) // Calculate rear clearance / 计算后向间距 [m]
        {
            if (VehicleLength != null)
            {
                FloatPoint[] pts;
                if (obj.PosMode == PositionMode.ClosestPoint) pts = new FloatPoint[1] { new FloatPoint((float)obj.PosX, (float)obj.PosY) };
                else pts = CalBoxCorners(obj, boxCornerRatio);

                double maxx = double.NegativeInfinity;
                for (int i = 0; i < pts.Length; i++)
                {
                    var pt = pts[i];
                    if (pt.X > maxx) maxx = pt.X;
                }

                if (maxx > 0) return null;
                else return -VehicleLength.Value - maxx;
            }
            else return null;
        }

        public static FloatPoint[] CalBoxCorners(ObjectInfo obj, double k) // 8 points are connected sequentially, and k is the reduction ratio of inclined points (generally 0.9~1) / 8个点顺序连接, k为斜方向点缩小比例(一般为0.9~1)
        {
            double posx = obj.PosX;
            double posy = obj.PosY;
            double width = obj.Width.Value;
            double length = obj.Length.Value;
            double heading = obj.Heading != null ? obj.Heading.Value : 0;
            float coef = (float)k;

            var pts = new FloatPoint[8];
            pts[0].X = (float)(length / 2); pts[0].Y = 0;
            pts[1].X = (float)(length / 2) * coef; pts[1].Y = (float)(width / 2) * coef;
            pts[2].X = 0; pts[2].Y = (float)(width / 2);
            pts[3].X = (float)(-length / 2) * coef; pts[3].Y = (float)(width / 2) * coef;
            pts[4].X = (float)(-length / 2); pts[4].Y = 0;
            pts[5].X = (float)(-length / 2) * coef; pts[5].Y = (float)(-width / 2) * coef;
            pts[6].X = 0; pts[6].Y = (float)(-width / 2);
            pts[7].X = (float)(length / 2) * coef; pts[7].Y = (float)(-width / 2) * coef;

            var deg2rad = Math.PI / 180;
            double coshd = Math.Cos(heading * deg2rad);
            double sinhd = Math.Sin(heading * deg2rad);
            for (int i = 0; i < 8; i++)
            {
                var pt = pts[i];
                double x = coshd * pt.X - sinhd * pt.Y;
                double y = sinhd * pt.X + coshd * pt.Y;
                pts[i].X = (float)(posx + x);
                pts[i].Y = (float)(posy + y);
            }
            return pts;
        }

        private double? interpolateAngle(double? a1, double w1, double? a2, double w2)
        {
            if (a1 == null || a2 == null) return null;

            var deg2rad = Math.PI / 180;
            var x1 = Math.Cos(a1.Value * deg2rad);
            var y1 = Math.Sin(a1.Value * deg2rad);
            var x2 = Math.Cos(a2.Value * deg2rad);
            var y2 = Math.Sin(a2.Value * deg2rad);

            var xo = x1 * w1 + x2 * w2;
            var yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return null;

            return Math.Atan2(yo, xo) / deg2rad;
        }
    }
}
